#include "Cube.h"
#include <assert.h>

#include <osg/Geode>
#include <osg/Geometry>
#include <osg/ShapeDrawable>
#include <osg/MatrixTransform>

#include <osg/Depth>
#include <osg/Material>
#include <osg/CullFace>
#include <osg/Point>
#include <osg/LineWidth>
#include <osg/Texture2D>
#include <osg/BlendFunc>
#include <osg/BlendColor>
#include <osg/PolygonMode>

#include <osg/Program>
#include <osgDB/ReadFile>

//-----------------------------------------------------------------------------
// local function
//-----------------------------------------------------------------------------

enum FaceName
{
    R_FACE = 0,
    L_FACE,
    B_FACE,
    F_FACE,
    U_FACE,
    D_FACE,
    NUM_FACE
};

static std::string nameTable = "rlbfud";
static std::string nameTable2 = "RLBFUD";

static std::vector<osg::Vec4> colorTable = {
    osg::Vec4(1.0f,0.0f,0.0f,1.0f), // +x, right
    osg::Vec4(1.0f,0.5f,0.0f,1.0f), // -x, left
    osg::Vec4(0.0f,0.0f,1.0f,1.0f), // +y, back
    osg::Vec4(0.0f,0.5f,0.0f,1.0f), // -y, front
    osg::Vec4(1.0f,1.0f,1.0f,1.0f), // +z, up
    osg::Vec4(1.0f,1.0f,0.0f,1.0f), // -z, down
};

static std::vector<osg::Vec3> normalTable = {
    osg::Vec3( 1, 0, 0), // +x, right
    osg::Vec3(-1, 0, 0), // -x, left
    osg::Vec3( 0, 1, 0), // +y, back
    osg::Vec3( 0,-1, 0), // -y, front
    osg::Vec3( 0, 0, 1), // +z, up
    osg::Vec3( 0, 0,-1), // -z, down
};

static std::vector< std::vector<osg::Vec3> > vertTable = {
    { osg::Vec3( 0.5f,-0.5f,-0.5f), osg::Vec3( 0.5f, 0.5f,-0.5f), osg::Vec3( 0.5f, 0.5f, 0.5f), osg::Vec3( 0.5f,-0.5f, 0.5f) }, // +x, right
    { osg::Vec3(-0.5f, 0.5f,-0.5f), osg::Vec3(-0.5f,-0.5f,-0.5f), osg::Vec3(-0.5f,-0.5f, 0.5f), osg::Vec3(-0.5f, 0.5f, 0.5f) }, // -x, left
    { osg::Vec3( 0.5f, 0.5f,-0.5f), osg::Vec3(-0.5f, 0.5f,-0.5f), osg::Vec3(-0.5f, 0.5f, 0.5f), osg::Vec3( 0.5f, 0.5f, 0.5f) }, // +y, back
    { osg::Vec3(-0.5f,-0.5f,-0.5f), osg::Vec3( 0.5f,-0.5f,-0.5f), osg::Vec3( 0.5f,-0.5f, 0.5f), osg::Vec3(-0.5f,-0.5f, 0.5f) }, // -y, front
    { osg::Vec3(-0.5f,-0.5f, 0.5f), osg::Vec3( 0.5f,-0.5f, 0.5f), osg::Vec3( 0.5f, 0.5f, 0.5f), osg::Vec3(-0.5f, 0.5f, 0.5f) }, // +z, up
    { osg::Vec3( 0.5f,-0.5f,-0.5f), osg::Vec3(-0.5f,-0.5f,-0.5f), osg::Vec3(-0.5f, 0.5f,-0.5f), osg::Vec3( 0.5f, 0.5f,-0.5f) }, // -z, down
};

static std::vector<osg::Quat> rotateTable = 
{
    osg::Quat(osg::PI_2, osg::Vec3( 1, 0, 0)), // +x, right
    osg::Quat(osg::PI_2, osg::Vec3(-1, 0, 0)), // -x, left
    osg::Quat(osg::PI_2, osg::Vec3( 0, 1, 0)), // +y, back
    osg::Quat(osg::PI_2, osg::Vec3( 0,-1, 0)), // -y, front
    osg::Quat(osg::PI_2, osg::Vec3( 0, 0, 1)), // +z, up
    osg::Quat(osg::PI_2, osg::Vec3( 0, 0,-1)), // -z, down
};

//    ______________
//   /             /|
//  /_____________/ |
// 3|            2| |     +z
//  |             | |     |   
//  |             | |     |  /+y
//  |             | /     | /
// 0|____________1|/      |/________+x
//

static osg::MatrixTransform* createBox(osg::Vec3i pos)
{
    osg::ref_ptr<osg::Vec3Array> verts = new osg::Vec3Array;
    osg::ref_ptr<osg::Vec3Array> norms = new osg::Vec3Array;
    osg::ref_ptr<osg::Vec2Array> coords = new osg::Vec2Array;
    osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
    
    osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;
    geom->setVertexArray(verts);
    geom->setNormalArray(norms, osg::Array::BIND_PER_PRIMITIVE_SET);
    geom->setTexCoordArray(0, coords, osg::Array::BIND_PER_VERTEX);
    geom->setColorArray(colors, osg::Array::BIND_PER_PRIMITIVE_SET);

    int offset = 0;
    osg::Vec3 c(pos.x(), pos.y(), pos.z());
    for (int i=0; i<6; ++i)
    {
        verts->push_back(c + vertTable[i][0] * 0.99f);
        verts->push_back(c + vertTable[i][1] * 0.99f);
        verts->push_back(c + vertTable[i][2] * 0.99f);
        verts->push_back(c + vertTable[i][3] * 0.99f);

        coords->push_back(osg::Vec2(0, 0));
        coords->push_back(osg::Vec2(1, 0));
        coords->push_back(osg::Vec2(1, 1));
        coords->push_back(osg::Vec2(0, 1));

        norms->push_back(normalTable[i]);
        colors->push_back(colorTable[i]);

        osg::ref_ptr<osg::DrawElementsUByte> face = new osg::DrawElementsUByte(GL_TRIANGLES);
        face->addElement(offset + 0);
        face->addElement(offset + 1);
        face->addElement(offset + 2);
        face->addElement(offset + 0);
        face->addElement(offset + 2);
        face->addElement(offset + 3);
        geom->addPrimitiveSet(face);
        offset += 4;
    }

    osg::ref_ptr<osg::MatrixTransform> mt = new osg::MatrixTransform;
    mt->addChild(geom);

    return mt.release();
}

//-----------------------------------------------------------------------------
// class Cube
//-----------------------------------------------------------------------------

class CubeUpdateCallback : public osg::NodeCallback
{
    double _lst;
public:
    CubeUpdateCallback() : _lst(0) {}
    void operator()(osg::Node* node, osg::NodeVisitor* nv) override 
    {
        Cube* cube = dynamic_cast<Cube*>(node);
        if (cube)
        {

            double cur = nv->getFrameStamp()->getReferenceTime();
            cube->handleAnimateIfNeeded(cur - _lst);
            _lst = cur;
        }
        traverse(node, nv);
    }
};

//-----------------------------------------------------------------------------
// class Cube
//-----------------------------------------------------------------------------

Cube::Cube()
{
    _duration = 0.5f;

    build();
}

Cube::~Cube()
{
    removeUpdateCallback(getUpdateCallback());
}

void Cube::build() 
{
    for (int i = -1; i <= 1; i++)
    {
        for (int j = -1; j <= 1; j++)
        {
            for (int k = -1; k <= 1; k++)
            {
                TransformPtr mt = createBox(osg::Vec3i(i, j, k));
                this->addChild(mt);
                _cube[i + 1][j + 1][k + 1] = mt;
            }
        }
    }
    
    this->addUpdateCallback(new CubeUpdateCallback);

    osg::ref_ptr<osg::Program> program = new osg::Program;
    program->addShader(osgDB::readShaderFile(osg::Shader::VERTEX, "cube_vs.glsl"));
    program->addShader(osgDB::readShaderFile(osg::Shader::FRAGMENT, "cube_fs.glsl"));
    getOrCreateStateSet()->setAttributeAndModes(program);
    osg::ref_ptr<osg::Texture2D> tex = new osg::Texture2D;
    tex->setImage(osgDB::readImageFile("pic.jpg"));
    getOrCreateStateSet()->setTextureAttributeAndModes(0, tex);
    getOrCreateStateSet()->addUniform(new osg::Uniform("tex", 0));

    osg::ref_ptr<osg::Texture2D> tex2 = new osg::Texture2D;
    tex2->setImage(osgDB::readImageFile("pic_norm.jpg"));
    getOrCreateStateSet()->setTextureAttributeAndModes(1, tex2);
    getOrCreateStateSet()->addUniform(new osg::Uniform("normTex", 1));
}

void Cube::handleAnimateIfNeeded(double dt)
{
    if (_animList.empty())
        return;
    
    auto& animData = *_animList.begin();
    
    int sign = animData.ccw ? 1 : -1;

    if (animData.run == 0)
    {
        for (int i = 0; i < 9; ++i)
        {
            animData.origin[i] = animData.group[i]->getMatrix();
        }
    }

    animData.run += dt;

    if (animData.run >= animData.duration)
    {
        osg::Matrix m;
        m.postMultTranslate(-animData.normal);
        m.postMultRotate(osg::Quat(sign * osg::PI_2f, animData.normal));
        m.postMultTranslate(animData.normal);
        for (int i = 0; i < 9; i++)
        {
            animData.group[i]->setMatrix(animData.origin[i] * m);
        }

        _animList.pop_front();
        return;
    }

    float rad = sign * osg::PI_2f * dt / animData.duration;
    osg::Quat q(rad, animData.normal);

    osg::Matrix m;
    m.postMultTranslate(-animData.normal);
    m.postMultRotate(q);
    m.postMultTranslate(animData.normal);
    for (auto& node : animData.group)
    {
        node->setMatrix(node->getMatrix() * m);
    }
}

void Cube::pickAndRotateGroup(CubeAnimateData& data)
{
    // ___________________     ___________________
    // |  1  |  2  |  3  |     |     |     |     |
    // |(0,2)|(1,2)|(2,2)|     |  3  |  6  |  9  |
    // |_____|_____|_____|     |_____|_____|_____|
    // |  4  |  5  |  6  | ccw |     |     |     |
    // |(0,1)|(1,1)|(2,1)| ==> |  2  |  5  |  8  |
    // |_____|_____|_____|     |_____|_____|_____|
    // |  7  |  8  |  9  |     |     |     |     |
    // |(0,0)|(1,0)|(2,0)|     |  1  |  4  |  7  |
    // |_____|_____|_____|     |_____|_____|_____|
    
    bool ccw = data.ccw;
    int face = data.face;
    auto& group = data.group;

    switch (face)
    {
    case R_FACE: 
    case L_FACE:
    {
        int i = (face == R_FACE) ? 2 : 0;
        for (int j = 0; j < 3; ++j)
            for (int k = 0; k < 3; ++k)
                group[3 * j + k] = _cube[i][j][k];
        
        if (ccw)
        {
            auto tmp = _cube[i][0][0];
            _cube[i][0][0] = _cube[i][0][2];
            _cube[i][0][2] = _cube[i][2][2];
            _cube[i][2][2] = _cube[i][2][0];
            _cube[i][2][0] = tmp;
            tmp = _cube[i][1][0];
            _cube[i][1][0] = _cube[i][0][1];
            _cube[i][0][1] = _cube[i][1][2];
            _cube[i][1][2] = _cube[i][2][1];
            _cube[i][2][1] = tmp;
        }
        else
        {
            auto tmp = _cube[i][0][0];
            _cube[i][0][0] = _cube[i][2][0];
            _cube[i][2][0] = _cube[i][2][2];
            _cube[i][2][2] = _cube[i][0][2];
            _cube[i][0][2] = tmp;
            tmp = _cube[i][1][0];
            _cube[i][1][0] = _cube[i][2][1];
            _cube[i][2][1] = _cube[i][1][2];
            _cube[i][1][2] = _cube[i][0][1];
            _cube[i][0][1] = tmp;
        }
    } break;
    case B_FACE:
    case F_FACE: 
    {
        int j = (face == B_FACE) ? 2 : 0;
        for (int i = 0; i < 3; ++i)
            for (int k = 0; k < 3; ++k)
                group[3 * i + k] = _cube[i][j][k];

        if (ccw)
        {
            auto tmp = _cube[0][j][0];
            _cube[0][j][0] = _cube[0][j][2];
            _cube[0][j][2] = _cube[2][j][2];
            _cube[2][j][2] = _cube[2][j][0];
            _cube[2][j][0] = tmp;
            tmp = _cube[1][j][0];
            _cube[1][j][0] = _cube[0][j][1];
            _cube[0][j][1] = _cube[1][j][2];
            _cube[1][j][2] = _cube[2][j][1];
            _cube[2][j][1] = tmp;
        }
        else
        {
            auto tmp = _cube[0][j][0];
            _cube[0][j][0] = _cube[2][j][0];
            _cube[2][j][0] = _cube[2][j][2];
            _cube[2][j][2] = _cube[0][j][2];
            _cube[0][j][2] = tmp;
            tmp = _cube[1][j][0];
            _cube[1][j][0] = _cube[2][j][1];
            _cube[2][j][1] = _cube[1][j][2];
            _cube[1][j][2] = _cube[0][j][1];
            _cube[0][j][1] = tmp;
        }
    } break;
    case U_FACE:
    case D_FACE: 
    {
        int k = (face == U_FACE) ? 2 : 0;
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                group[3 * i + j] = _cube[i][j][k];

        if (ccw)
        {
            auto tmp = _cube[0][0][k];
            _cube[0][0][k] = _cube[0][2][k];
            _cube[0][2][k] = _cube[2][2][k];
            _cube[2][2][k] = _cube[2][0][k];
            _cube[2][0][k] = tmp;
            tmp = _cube[1][0][k];
            _cube[1][0][k] = _cube[0][1][k];
            _cube[0][1][k] = _cube[1][2][k];
            _cube[1][2][k] = _cube[2][1][k];
            _cube[2][1][k] = tmp;
        }
        else
        {
            auto tmp = _cube[0][0][k];
            _cube[0][0][k] = _cube[2][0][k];
            _cube[2][0][k] = _cube[2][2][k];
            _cube[2][2][k] = _cube[0][2][k];
            _cube[0][2][k] = tmp;
            tmp = _cube[1][0][k];
            _cube[1][0][k] = _cube[2][1][k];
            _cube[2][1][k] = _cube[1][2][k];
            _cube[1][2][k] = _cube[0][1][k];
            _cube[0][1][k] = tmp;
        }
    } break;
    default:
        break;
    }
}

void Cube::front(bool ccw)
{
    CubeAnimateData data;
    data.face = F_FACE;
    data.run = 0;
    data.ccw = ccw;
    data.duration = duration();
    data.normal = normalTable[F_FACE];
    pickAndRotateGroup(data);
    _animList.push_back(data);
    _state.append(1, ccw ? 'f' : 'F');
}

void Cube::back(bool ccw) 
{
    CubeAnimateData data;
    data.face = B_FACE;
    data.run = 0;
    data.ccw = ccw;
    data.duration = duration();
    data.normal = normalTable[F_FACE];
    pickAndRotateGroup(data);
    _animList.push_back(data);
    _state.append(1, ccw ? 'b' : 'B');
}

void Cube::right(bool ccw) 
{
    CubeAnimateData data;
    data.face = R_FACE;
    data.run = 0;
    data.ccw = ccw;
    data.duration = duration();
    data.normal = normalTable[R_FACE];
    pickAndRotateGroup(data);
    _animList.push_back(data);
    _state.append(1, ccw ? 'r' : 'R');
}

void Cube::left(bool ccw) 
{
    CubeAnimateData data;
    data.face = L_FACE;
    data.run = 0;
    data.ccw = ccw;
    data.duration = duration();
    data.normal = normalTable[R_FACE];
    pickAndRotateGroup(data);
    _animList.push_back(data);
    _state.append(1, ccw ? 'l' : 'L');
}

void Cube::up(bool ccw) 
{
    CubeAnimateData data;
    data.face = U_FACE;
    data.run = 0;
    data.ccw = ccw;
    data.duration = duration();
    data.normal = normalTable[U_FACE];
    pickAndRotateGroup(data);
    _animList.push_back(data);
    _state.append(1, ccw ? 'u' : 'U');
}

void Cube::down(bool ccw) 
{
    CubeAnimateData data;
    data.face = D_FACE;
    data.run = 0;
    data.ccw = ccw;
    data.duration = duration();
    data.normal = normalTable[U_FACE];
    pickAndRotateGroup(data);
    _animList.push_back(data);
    _state.append(1, ccw ? 'd' : 'D');
}

std::string Cube::writeState()
{
    return _state;
}

void Cube::readState(const std::string& state) 
{
    reset();

    for (size_t i = 0; i < state.size(); i++)
    {
        switch (state[i])
        {
        case 'f': front(); break;
        case 'b': back(); break;
        case 'l': left(); break;
        case 'r': right(); break;
        case 'u': up(); break;
        case 'd': down(); break;
        case 'F': front(false); break;
        case 'B': back(false); break;
        case 'L': left(false); break;
        case 'R': right(false); break;
        case 'U': up(false); break;
        case 'D': down(false); break;
        default:
            break;
        }
    }
}

void Cube::reset()
{
    _state = "";
    _animList.clear();
    for (unsigned int n = 0; n < getNumChildren(); n++)
    {
        osg::MatrixTransform* mt = dynamic_cast<osg::MatrixTransform*>(getChild(n));
        mt->setMatrix(osg::Matrix::identity());
        int k = n % 3;
        int j = ((n - k) / 3) % 3;
        int i = ((n - 3 * j - k) / 9) % 3;
        _cube[i][j][k] = mt;
    }
}

void Cube::reverse() 
{
    std::string restr;
    int size = _state.size();
    restr.resize(size);
    for (int i = 0; i < size; i++)
    {
        if (isupper(_state[i]))
            restr[size - 1 - i] = tolower(_state[i]);
        else 
            restr[size - 1 - i] = toupper(_state[i]);
    }

    for (int i = 0; i < size; i++)
    {
        switch (restr[i])
        {
        case 'f': front(); break;
        case 'b': back(); break;
        case 'l': left(); break;
        case 'r': right(); break;
        case 'u': up(); break;
        case 'd': down(); break;
        case 'F': front(false); break;
        case 'B': back(false); break;
        case 'L': left(false); break;
        case 'R': right(false); break;
        case 'U': up(false); break;
        case 'D': down(false); break;
        default:
            break;
        }
    }

    _state = "";
}

void Cube::solve() 
{
    assert(false);
}